//
// Created by xuwj on 2021/10/25.
//

#ifndef NAV_ORB_TRACK_ROSITEMS_H
#define NAV_ORB_TRACK_ROSITEMS_H

#include "ros/ros.h"
#include "cv_bridge/cv_bridge.h"
#include "geometry_msgs/PoseStamped.h"
#include "nav_msgs/Odometry.h"

#include "opencv2/opencv.hpp"

namespace TagItems {

using namespace std;

struct TagOdomData{
    double timestamp_;
    cv::Mat Tcam2map;
};

class PosePublisher {
public:
    PosePublisher(ros::NodeHandle &nh, string topic_name, int buff_size, string frame_id) ;

    void publishPose(cv::Mat &mPose);

private:
    ros::Publisher publisher_;
//    string topic_name_;
    string frame_id_;
    static unsigned long seq_;
};

class OdomSubscriber{
public:
    OdomSubscriber(ros::NodeHandle &nh, string topic_name, int buff_size, string frame_id, int data_quantity);

    void OdomMsgCallback(const nav_msgs::Odometry::ConstPtr &odom_msg);

    void ParseTagData(deque<TagOdomData>& tagOdom_data_buf);

private:
    ros::Subscriber subscriber_;
    string topic_name_;
    string frame_id_;
    static unsigned long seq_;

    int data_quantity_limit;

    deque<TagOdomData> new_TagOdom_data_;

};

}



#endif //NAV_ORB_TRACK_ROSITEMS_H
